#ifndef MESH_H
#define MESH_H
#include <map>
#include <hash_map>
#include <fstream>

class HalfEdge ;
class Edge;
class Face;
class Vertex; 

class HalfEdge 
{
public:
	HalfEdge * next, * prev, * flip;
	Face * face;
	Vertex * from_v ;
	Edge * edge; 
};

class Face 
{
public: 
	HalfEdge * he;
} ;
class Point 
{
public:
	Point () {}
	Point (float a, float b, float c)
	{
		x = a; y = b; z = c;
	}
	float x, y, z;
	friend float operator * (Point & lhs, Point & rhs)
	{
		return (lhs.x * rhs.x + lhs.y * rhs.y + lhs.z * rhs.z);
	}
	friend Point operator + (Point & lhs, Point & rhs)
	{
		return Point (lhs.x + rhs.x , lhs.y + rhs.y , lhs.z + rhs.z);
	}
	friend Point operator / (Point & lhs, float rhs)
	{
		return Point (lhs.x / rhs, lhs.y / rhs, lhs.z / rhs);
	}
	friend Point operator - (Point & lhs, Point & rhs)
	{
		return Point (lhs.x - rhs.x , lhs.y - rhs.y , lhs.z - rhs.z);
	}
	friend Point operator ^ (Point & lhs, Point & rhs)
	{
		return Point (lhs.y * rhs.z - lhs.z * rhs.y, 
					lhs.z * rhs.x - lhs.x * rhs.z,
					lhs.x * rhs.y - lhs.y * rhs.x );
	}
	static Point normal (Point & p)
	{
		float length = sqrt (p.x * p.x + p.y * p.y + p.z * p.z);
		return Point (p.x /length, p.y /length, p.z /length);
	}
};
class Vertex
{
public:
	Vertex () {}
	class VVIter 
	{
	public:
		VVIter (Vertex * v)
		{
			he = v->edge; 
		};
		VVIter (HalfEdge * edge)
		{
			he = edge; 
		}
		VVIter & operator ++()
		{
			this->he = he->flip->next ;
			return (*this) ;
		}
		Vertex * operator*()
		{
			return (this->he->flip->from_v);
		}
		bool operator ==(const VVIter & other)
		{
			return(he == other.he);
		}

		bool operator!=(const VVIter & other)
		{
			return(he != other.he);
		}
	private :
		HalfEdge * he;  
	};
public:
	VVIter vvbegin ()
	{
		return VVIter (this) ;
	}
	VVIter vvend ()
	{
		return VVIter (this);
	}
public: 
	Point point; 
	HalfEdge * edge; 
	unsigned idx ;

	unsigned num_neigh ;
	Point sum_neigh ;
	float qua_sum_neigh ; 

};
class Edge 
{
public :
	HalfEdge * he ; 
	friend std::ostream & operator << (std::ostream & ostr, Edge * e)
	{
		ostr <<'('<<e->he->from_v->idx<<','; 
		ostr <<e->he->flip->from_v->idx<<')';
		return ostr;
	}
	float cost; 
};

typedef std::pair<int, int> HashKey;
typedef HashKey HalfEdgeHashKey ;
typedef HashKey EdgeHashKey; 

template<>
struct stdext::hash_compare<HashKey > {
    // the mean number of elements per bucket
    static const size_t bucket_size = 4;
    // the minimum number of buckets
    static const size_t min_buckets = 65535*16;
    // hash function
    size_t operator()(const HashKey & key) const {
        return (key.first * key.second);
    }
    // compare function
    bool operator()(const HashKey & lhs, const HashKey & rhs) const {
        return lhs < rhs;
    }
};

class FaceHashKey
{
public:
	FaceHashKey(unsigned a, unsigned b, unsigned c)
	{
		idx1 = a; idx2 = b; idx3 = c;
	}
public:
	unsigned idx1, idx2, idx3 ;
};

template<>
struct stdext::hash_compare<FaceHashKey > {
    // the mean number of elements per bucket
    static const size_t bucket_size = 4;
    // the minimum number of buckets
    static const size_t min_buckets = 65535*2;
    // hash function
    size_t operator()(const FaceHashKey & key) const {
		return key.idx1 * key.idx2 + key.idx3 ;
    }
    // compare function
    bool operator()(const FaceHashKey & lhs, const FaceHashKey & rhs) const {
		return (lhs.idx1 < rhs.idx1 || 
			(lhs.idx1 == rhs.idx1 && lhs.idx2 < rhs.idx2) ||
			(lhs.idx1 == rhs.idx1 && lhs.idx2 == rhs.idx2 && lhs.idx3 < rhs.idx3)) ;
    }
};
inline HalfEdgeHashKey halfEdgeHashKey (HalfEdge * he)
{
	unsigned id1 = he->from_v->idx ;
	unsigned id2 = he->next->from_v->idx; 
	return std::make_pair (id1, id2);
}
inline EdgeHashKey edgeHashKey (unsigned id1, unsigned id2)
{
	return (id1 < id2 ? std::make_pair(id1, id2) : std::make_pair(id2, id1)) ;
}
inline EdgeHashKey edgeHashKey(Edge * edge)
{
	if (edge == NULL) 
	{
		std::cout<<"INVALID EDGE"<<std::endl;
	}
	unsigned id1 = edge->he->from_v->idx;
	unsigned id2 = edge->he->next->from_v->idx ;
	return edgeHashKey(id1, id2);
}
inline FaceHashKey faceHashKey (unsigned id1, unsigned id2, unsigned id3)
{
	if (id1 < id2  && id1 < id3) return FaceHashKey(id1, id2, id3 );
	if (id2 < id1  && id2 < id3) return FaceHashKey(id2, id3, id1 );
	if (id3 < id1  && id3 < id2) return FaceHashKey(id3, id1, id2 );
}

inline FaceHashKey faceHashKey (Face * face)
{
	unsigned id1 = face->he->from_v->idx; 
	unsigned id2 = face->he->next->from_v->idx ;
	unsigned id3 = face->he->next->next->from_v->idx ;

	return faceHashKey (id1, id2, id3 );
}
inline void adjust_vertex (Vertex * v1, Vertex * v2)
{
	v1->point = (v1->sum_neigh + v2->sum_neigh) / float(v1->num_neigh + v2->num_neigh); 
	v1->num_neigh +=  v2->num_neigh;
	v1->qua_sum_neigh += v2->qua_sum_neigh;
	v1->sum_neigh = v1->sum_neigh + v2->sum_neigh ;
}

inline bool is_edge_safe (Edge * edge)
{
	Vertex * v1 = edge->he->from_v ;
	Vertex * v2 = edge->he->next->from_v; 
	unsigned count = 0;
	Vertex::VVIter iter1 = v1->vvbegin ();
	Vertex::VVIter end1 = v1->vvend();
	do 
	{
		Vertex::VVIter iter2 = v2->vvbegin ();
		Vertex::VVIter end2 = v2->vvend();
		do
		{
			//std::cout<<"TEST"<<std::endl;
			if ( (*iter1)->idx == (*iter2)->idx ) count ++; 
			//std::cout<<(*iter2)->idx<<' ';
			++iter2;
		}while (iter2 != end2);
		++iter1;
	}while (iter1 != end1);
	return (2 == count);
}

typedef stdext::hash_map <EdgeHashKey, Edge * >::iterator EdgeIterator ;

class Mesh 
{
public: 
	friend std::istream & operator >> (std::istream & istr, Mesh & mesh)
	{
		std::string data; 
		Vertex * v; 
		unsigned count = 0; 
		while (istr >> data)
		{
			switch (data[0])
			{
			case 'v' :
				float x, y, z;
				istr >> x >> y >> z ;
				v = new Vertex;
				v->point.x = x ; v->point.y = y; v->point.z = z;
				v->edge = NULL; 
				v->idx = count ;
				mesh.vertices[count]=v;
				count ++ ;
				getline (istr, data);
				break;
			case 'f' :
				unsigned id1, id2, id3;
				istr >> id1 >> id2 >>id3 ;
				mesh.construct(id1-1, id2-1, id3-1);
				getline (istr, data );
				break;
			default :
				getline (istr, data);
			}
		}
		return istr;
	}
	friend std::ostream & operator << (std::ostream & ostr, Mesh & mesh)
	{
		for (unsigned i = 0 ;i < mesh.vertices.size(); ++i)
		{
			ostr <<"v " << mesh.vertices[i]->point.x <<' '<< mesh.vertices[i]->point.y <<' '<<mesh.vertices[i]->point.z<<std::endl;
		}
		/*
		for (unsigned i = 0 ;i < mesh.vertices.size(); ++i)
		{
			ostr <<"vt " << mesh.vertices[i]->tex_coord.x<<' '<< mesh.vertices[i]->tex_coord.y<<std::endl;
		}
		*/
		for (stdext::hash_map <FaceHashKey, Face * >::iterator iter =  mesh.faces.begin(); iter != mesh.faces.end(); ++ iter)
		{
			HalfEdge * he  = (*iter).second->he; 
			ostr <<"f "<<he->from_v->idx + 1<<'/'<<he->from_v->idx + 1<<' '
				<<he->next->from_v->idx + 1<<'/'<<he->next->from_v->idx + 1<<' '
				<<he->next->next->from_v->idx + 1<<'/'<<he->next->next->from_v->idx + 1<<std::endl;
		}
		return ostr; 
	}
	~Mesh()
	{
		for (stdext::hash_map <unsigned, Vertex * >::iterator it = vertices.begin();
			it != vertices.end(); ++it)
		{
			if (it->second) delete (it->second);
			it->second = NULL; 			
		}
		std::cerr<<"Remove vertices"<<std::endl;

		for (stdext::hash_map <FaceHashKey, Face * >::iterator it = faces.begin();
			it != faces.end(); ++it)
		{
			if (it->second) delete (it->second);
			it->second = NULL; 			
		}
		std::cerr<<"Remove faces"<<std::endl;
		
		for (stdext::hash_map <HashKey, Edge * >::iterator it = edges.begin(); 
			it !=edges.end(); ++it)
		{
			if (it->second) delete it->second; 
			it->second = NULL; 
		}
		std::cerr<<"Remove edges"<<std::endl;
		for (stdext::hash_map <HashKey, HalfEdge * >::iterator it = hash_he.begin(); 
			it != hash_he.end(); ++it)
		{
			if (it->second) delete it->second; 
			it->second = NULL; 
		}
		std::cerr<<"Remove Halfedges"<<std::endl;
	}


	void evaluate_edge_cost (Edge * edge)
	{
			Vertex * v1 = edge->he->from_v ;
			Vertex * v2 = edge->he->next->from_v ;

			Point p =  (v1->sum_neigh + v2->sum_neigh) / float(v1->num_neigh + v2->num_neigh) ;

			edge->cost = (v1->num_neigh + v2->num_neigh)*(p*p) - 
				2 * (p *((v1->sum_neigh + v2->sum_neigh))) 
				+ v1->qua_sum_neigh + v2->qua_sum_neigh;
	}
	void evaluate_edge_around_vertex(Vertex * v)
	{
		HalfEdge * iter = v->edge; 
		HalfEdge * end = iter;
		do
		{
			evaluate_edge_cost(iter->edge);
			iter = iter->flip->next ;
		} while(iter != end);
	}
	void prepare_simp_data()
	{
		for (stdext::hash_map <unsigned, Vertex * >::iterator v_iter = vertices.begin(); 
			v_iter != vertices.end(); ++v_iter)
		{
			unsigned count = 0;
			Point p(0.0, 0.0, 0.0);
			float sum = 0.0;
			Vertex::VVIter iter = (v_iter)->second->vvbegin();
			Vertex::VVIter end = (v_iter)->second->vvend();
			
			do 
			{
				++iter;
				++count;
				p = p + (*iter)->point; 
				sum += ((*iter)->point) * ((*iter)->point);
			} while (iter != end) ;
			v_iter->second->num_neigh = count;
			v_iter->second->sum_neigh = p; 
			v_iter->second->qua_sum_neigh = sum; 
		}
		for (stdext::hash_map <HashKey, Edge * >::iterator iter = edges.begin(); 
			iter != edges.end(); ++iter)
		{
			evaluate_edge_cost(iter->second);
		}
	}

	void edge_collapse (Edge * edge, unsigned & id_old , unsigned & id_new, std::vector<unsigned> & neighbor)
	{
		//std::cout<<"Enter Edge Collapse"<<std::endl;
		HalfEdge * he = edge->he;
		Vertex * v = he->from_v ;
		EdgeHashKey  rm_ek = edgeHashKey(edge) ;
		EdgeHashKey  rm_ek1 = edgeHashKey(he->next->edge) ;
		EdgeHashKey  rm_ek2 = edgeHashKey(he->flip->prev->edge) ;
		//std::cout<<"Remove Edge "<<edge <<he->next->edge << he->flip->prev->edge <<std::endl;

		//Adjust V position and cost 
		adjust_vertex(he->from_v, he->next->from_v);
		//Make two edges safe
		he->prev->edge->he = he->prev->flip;
		he->flip->next->edge->he = he->flip->next->flip;

		//std::cout<<"EDGE SAFE "<<he->prev->edge <<he->prev->edge->he->from_v->idx <<' '<<
		//he->prev->edge->he->next->from_v->idx	<<std::endl;
		//std::cout<<"EDGE SAFE "<<he->flip->next->edge<<he->flip->next->edge->he->from_v->idx <<' '<<
		//	he->flip->next->edge->he->next->from_v->idx<<std::endl;

		//Make three vertex safe
		he->from_v->edge = he->prev->flip ;
		he->prev->from_v->edge = he->prev->flip->next ;
		he->flip->next->flip->from_v->edge = he->flip->next->flip ;

		

		//-----------------------------------------------
		HalfEdge * begin = he->next;
		HalfEdge * end = he->flip ;
		HalfEdge * end2 = he->flip->prev->flip ;
		HalfEdge * iter = begin; 
		iter = iter->flip->next; 
		id_new = he->from_v->idx ;
		id_old = he->next->from_v->idx; 
		//std::cout<<id_new<<','<<id_old<<std::endl;
		while (iter != end)
		{
			unsigned id = iter->flip->from_v->idx; 
			if (iter != end && iter != end2)
			{
				//std::cout<<iter->flip->from_v->idx <<' ';
				Edge * edge_p = edges[edgeHashKey(id_old, id)];
				Edge * iter_p = iter->edge;
				//std::cout<<"EDGE "<<(void *)edge_p<<' '<<id_old<<','<<id<<std::endl;	
				//std::cout<<"ITER "<<(void *)iter_p<<' '<<id_old<<','<<id<<std::endl;
				//std::cout<<'['<<edge_p->he->from_v->idx <<' '<<edge_p->he->next->from_v->idx <<' '<<edge_p->he->next->next->from_v->idx <<']'<<std::endl;
				//std::cout<<'['<<iter_p->he->from_v->idx <<' '<<iter_p->he->next->from_v->idx <<' '<<iter_p->he->next->next->from_v->idx <<']'<<std::endl;
				
				edges[edgeHashKey(id_new, id)] =  iter->edge ;
				edges.erase(edgeHashKey(id_old, id));
				neighbor.push_back(id);
			}
			iter->from_v = he->from_v ;
			//std::cout<<'['<<iter->edge <<' '<<iter->next->edge <<' '<<iter->next->next->edge <<']'<<std::endl;
			//std::cout<<'['<<iter->from_v->idx <<' '<<iter->next->from_v->idx <<' '<<iter->next->next->from_v->idx <<']'<<std::endl;
			iter = iter->flip->next;
		}
		
		//Make two halfedge safe
		he->next->flip ->edge = he->prev->edge ;
		he->flip->prev->flip->edge = he->flip->next->edge; 
		//Flip Half edge
		he->prev->flip->flip = he->next->flip ;
		he->next->flip->flip = he->prev->flip ;
		he->flip->next->flip->flip  = he->flip->prev->flip;
		he->flip->prev->flip->flip = he->flip->next->flip;

		//std::cout<<std::endl;
		//this->print_vertex_neighbor(v);
		edges.erase(rm_ek) ;
		edges.erase(rm_ek1);
		edges.erase(rm_ek2);

		//Update the cost of edges around the new vertex 
		evaluate_edge_around_vertex(he->from_v);
		
	}
private:

	void construct (const unsigned id1, const unsigned id2, const unsigned id3)
	{
		Face * face = new Face; 
		HalfEdge * he12 = new HalfEdge ;
		HalfEdge * he23 = new HalfEdge ;
		HalfEdge * he31 = new HalfEdge ;
		he12->prev = he31 ; he12->next = he23; he12->face = face; he12->from_v = vertices[id1] ;
		he23->prev = he12 ; he23->next = he31; he23->face = face; he23->from_v = vertices[id2] ;
		he31->prev = he23 ; he31->next = he12; he31->face = face; he31->from_v = vertices[id3] ;

		hash_he[HashKey(id1 , id2)] = he12;
		hash_he[HashKey(id2 , id3)] = he23;
		hash_he[HashKey(id3 , id1)] = he31;
		
		vertices[id1]->edge = he12; 
		vertices[id2]->edge = he23; 
		vertices[id3]->edge = he31; 

		face->he =  he12 ;

		faces[faceHashKey(id1, id2, id3)] = face;

		stdext::hash_map <HashKey, HalfEdge * >::const_iterator iter; 
		//Update the flips and Edges; 
		iter = hash_he.find(HashKey(id2, id1)) ;
		if (hash_he.end() == iter)
		{
			he12->flip = NULL;
			Edge * edge = new Edge;
			edge->he = he12 ;
			he12->edge = edge;
			if (id1 < id2) edges[HashKey(id1, id2)] = edge;
			else edges[HashKey(id2, id1)] = edge;
		}
		else
		{
			he12->flip = (*iter).second;
			(*iter).second->flip = he12 ;
			he12->edge = (*iter).second->edge ;
		}
		iter = hash_he.find(HashKey(id3, id2)) ;
		if (hash_he.end() == iter)
		{
			he23->flip = NULL;
			Edge * edge = new Edge;
			edge->he = he23 ;
			he23->edge = edge;
			if (id2 < id3) edges[HashKey(id2, id3)] = edge;
			else edges[HashKey(id3, id2)] = edge;
		}
		else
		{
			he23->flip = (*iter).second;
			(*iter).second->flip = he23 ;
			he23->edge = (*iter).second->edge ;
		}
		iter = hash_he.find(HashKey(id1, id3)) ;
		if (hash_he.end() == iter)
		{
			he31->flip = NULL;
			Edge * edge = new Edge;
			edge->he = he31 ;
			he31->edge = edge;
			if (id3 < id1) edges[HashKey(id3, id1)] = edge;
			else edges[HashKey(id1, id3)] = edge;
		}
		else
		{
			he31->flip = (*iter).second;
			(*iter).second->flip = he31 ;
			he31->edge = (*iter).second->edge ;
		}
	}
public:
	void print_halfedges ()
	{
		std::cout<<"HALF EDGE SIZE :"<<edges.size()*2<<std::endl;
		for (stdext::hash_map <HashKey, Edge * >::iterator iter = edges.begin(); 
			iter != edges.end(); ++iter )
		{
			HalfEdge * he1 = iter->second->he ;
			HalfEdge * he2 = iter->second->he->flip; 
			if (he1->next->next->next != he1) std::cout<<"Invalid Halfedge"<<std::endl;
			if (he2->next->next->next != he2) std::cout<<"Invalid Halfedge"<<std::endl;
			std::cout<<'['<<he1->from_v->idx<<','<<he1->next->from_v->idx<<']'<<std::endl;
			std::cout<<'['<<he2->from_v->idx<<','<<he2->next->from_v->idx<<']'<<std::endl;
		}
	}
	void print_vertex_neighbor(Vertex * v)
	{
		std::cout<<std::endl<<"VERTEX NEIGHBOR : "<<v->idx<<std::endl;
		HalfEdge * iter = v->edge; 
		HalfEdge * end = iter; 
		do
		{
			std::cout<<'['<<iter->from_v->idx <<' '<<iter->next->from_v->idx <<' '<<iter->next->next->from_v->idx <<']'<<std::endl;
			iter = iter->flip->next ;
		}while (iter != end);
		
	}
	void print_edges ()
	{
		std::cout<<"EDGE SIZE :"<<edges.size()<<std::endl;
		for (stdext::hash_map <HashKey, Edge * >::iterator iter = edges.begin(); 
			iter != edges.end(); ++iter )
		{
			
			std::cout<<'['<<iter->first.first<<','<<iter->first.second<<']'<<iter->second; 
			if (iter->first != edgeHashKey(iter->second->he->from_v->idx, iter->second->he->next->from_v->idx ))
			{
				std::cout<<" INVALID EDGE--";
				std::cout<<'['<<iter->first.first<<','<<iter->first.second<<']'<<
					iter->second->he->from_v->idx<<','<<iter->second->he->next->from_v->idx <<std::endl;
				exit(0);
			}
			std::cout<<std::endl;
		}
	}
	void print_vertex ()
	{
		for (stdext::hash_map <unsigned, Vertex * >::iterator iter = vertices.begin ();
			iter != vertices.end(); ++iter )
		{
			std::cout<<iter->second->idx << ": ("<< iter->second->num_neigh<<')';

			Vertex::VVIter iter_in = iter->second->vvbegin();
			Vertex::VVIter end = iter->second->vvend();
			do 
			{
				std::cout<<(*iter_in)->idx<<' ';
				++iter_in;
			} while (iter_in != end) ;
			std::cout<<std::endl;
			
		}
	}

	bool is_valid_mesh()
	{
		for (stdext::hash_map <HashKey, Edge * >::iterator iter = edges.begin(); 
			iter != edges.end(); ++iter )
		{
			HalfEdge * he1 = iter->second->he ;
			HalfEdge * he2 = iter->second->he->flip; 
			if (he1->next->next->next != he1)
			{
				std::cout<<"INVALID EDGE [CIRCLE1]:";
				return false;
			}
			if (he2->next->next->next != he2)
			{
				std::cout<<"INVALID EDGE [CIRCLE2]:";
				return false;
			}

			if (he1->next->from_v != he1->flip->from_v )
			{
				std::cout<<"\nINVALID EDGE [NEXT<->FLIP]1:"<<std::endl;
				print_vertex_neighbor(he1->from_v);
				print_vertex_neighbor(he1->next->from_v);
				print_vertex_neighbor(he1->flip->from_v);
				std::cout<<"NEXT "<<he1->next->from_v->idx<<' '<<he1->next->next->from_v->idx
					<<' '<<he1->next->next->next->from_v->idx<<std::endl;
				std::cout<<"FLIP "<<he1->flip->from_v->idx<<std::endl;
				return false;
			}
			if (he2->next->from_v != he2->flip->from_v )
			{
				std::cout<<"\nINVALID EDGE [NEXT<->FLIP]2:";
				print_vertex_neighbor(he2->from_v);
				print_vertex_neighbor(he2->next->from_v);
				print_vertex_neighbor(he2->flip->from_v);
				return false;
			}
			if (iter->first != edgeHashKey(iter->second->he->from_v->idx, iter->second->he->next->from_v->idx ))
			{
				std::cout<<std::endl;
				std::cout<<"INVALID EDGE [NEXT]:";
				std::cout<<'['<<iter->first.first<<','<<iter->first.second<<']'<<
					iter->second->he->from_v->idx<<','<<iter->second->he->next->from_v->idx <<std::endl;
				return false;
			}
			if (iter->first != edgeHashKey(iter->second->he->from_v->idx, iter->second->he->flip->from_v->idx ))
			{
				std::cout<<std::endl;
				std::cout<<" INVALID EDGE [FLIP]:";
				std::cout<<'['<<iter->first.first<<','<<iter->first.second<<']'<<
					iter->second->he->from_v->idx<<','<<iter->second->he->flip->from_v->idx <<std::endl;
				return false;
			}
		}
		return true;
	}
public:
	stdext::hash_map <unsigned, Vertex * > vertices;
	//std::vector <Vertex * > vertices; 
	stdext::hash_map <HashKey, Edge * > edges; 
	stdext::hash_map <FaceHashKey, Face * >  faces; 
	stdext::hash_map <HashKey, HalfEdge * > hash_he; 
};

#endif